﻿#include "dragctrl.h"

using namespace DRAGCTRL;


DragCtrl::DragCtrl(void *_sourceOfRotMatrix) : sourceOfRotMatrix(_sourceOfRotMatrix)
,powerBase(1.5)
{
    memset(actuatorVel,0,sizeof(actuatorVel));
    memset(forceOffSet,0,sizeof(forceOffSet));
}

DragCtrl::~DragCtrl()
{
}

bool DragCtrl::coordinatesTrans(float *F)
{
    if(rotM==NULL){
        return false;
    }else{
        //坐标系旋转变换
        //力
        F[0]=F[0]*rotM[0][0]+F[1]*rotM[0][1]+F[2]*rotM[0][2];
        F[1]=F[0]*rotM[1][0]+F[1]*rotM[1][1]+F[2]*rotM[1][2];
        F[2]=F[0]*rotM[2][0]+F[1]*rotM[2][1]+F[2]*rotM[2][2];

        //力矩
        F[3]=F[3]*rotM[0][0]+F[4]*rotM[0][1]+F[2]*rotM[0][2];
        F[4]=F[3]*rotM[1][0]+F[4]*rotM[1][1]+F[5]*rotM[1][2];
        F[5]=F[3]*rotM[2][0]+F[4]*rotM[2][1]+F[5]*rotM[2][2];

        return true;
    }
}

void DragCtrl::dragCal(float *F)
{
    calZeroOffSet(F);
    if(getRotMatrix(sourceOfRotMatrix)){
        coordinatesTrans(F);
    }

    if(F[0]>FMAX || F[1]>FMAX || F[2]>FMAX) sigForceOver();

    actuatorVel[0]= abs(F[0])>FfMAX ? sig(F[0])*KV*(1-pow(powerBase,FfMAX-sig(F[0])*F[0]))/M : 0.0;
    actuatorVel[1]= abs(F[1])>FfMAX ? sig(F[1])*KV*(1-pow(powerBase,FfMAX-sig(F[1])*F[1]))/M : 0.0;
    actuatorVel[2]= abs(F[2])>FfMAX ? sig(F[2])*KV*(1-pow(powerBase,FfMAX-sig(F[2])*F[2]))/M : 0.0;

    //角速度部分都设为0
    actuatorVel[3]=0;
    actuatorVel[4]=0;
    actuatorVel[5]=0;

    sigActutorVel(actuatorVel);
    //TODO: 通过逆雅可比计算各个关节对应速度
    //InverseJacobian(actuatorVel,jointsVel);
}

bool DragCtrl::setZeroOffSet(double *F)
{
    memcpy(forceOffSet,F,sizeof (forceOffSet));
    return 1;
}

bool DragCtrl::calZeroOffSet(float *FVal)
{
    for(int i=0; i<6; i++){
        FVal[i]-=forceOffSet[i];
    }
    return 1;
}

bool DragCtrl::getActuatorVel(double *actVel)
{
    memcpy(actVel,actuatorVel,sizeof(actuatorVel));
    return 1;
}

double DragCtrl::getPowerBase() const
{
    return powerBase;
}

void DragCtrl::setPowerBase(double value)
{
    powerBase = value;
}

double DragCtrl::sig(double val)
{
    if(0==val) return 0;
    else return val>0? 1.0 : -1.0;
}

void *DragCtrl::getSourceOfRotMatrix() const
{
    return sourceOfRotMatrix;
}

void DragCtrl::setSourceOfRotMatrix(void *value)
{
    sourceOfRotMatrix = value;
}




